#! /usr/bin/env python

PACKAGE_NAME="dynamic_robot_localization"
NODE_NAME="planar_localization"
FILE_NAME="PlanarLocalization"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# Subscribe topics
gen.add("pointcloud_topic", str_t, 1, "Topic that publishes planar point clouds in map frame", "planar_pointcloud")
gen.add("costmap_topic", str_t, 1, "Topic with the 2D costmap", "map")

# Publish topics
gen.add("reference_map_pointcloud_publish_topic", str_t, 1, "Topic where the point cloud created from the 2D costmap will be published (if empty, point cloud wont be published)", "reference_map_pointcloud")
gen.add("aligned_pointcloud_publish_topic", str_t, 1, "Topic where the point cloud aligned with the map will be published (if empty, point cloud wont be published)", "aligned_pointcloud")
gen.add("pose_publish_topic", str_t, 1, "Topic where the corrected pose will be published", "initialpose")

# Configurations
gen.add("publish_tf_map_odom", bool_t, 1, "If activated, the transform between map and odom coordiante frames will be published", True)
gen.add("add_odometry_displacement", bool_t, 1, "If activated, the odometry will be integrated into the pose estimation", False)
gen.add("base_link_frame_id", str_t, 1, "TF frame used for localization (directly connected to odom frame)", "base_link")
gen.add("max_seconds_scan_age", double_t, 1, "Maximum allowed age of a scan (in seconds)", 5.0, 0.001)
gen.add("min_seconds_between_laserscan_registration", double_t, 1, "Minimum number of seconds to wait before performing pose correction", 0.05, 0.001)
gen.add("min_seconds_between_map_update", double_t, 1, "Minimum number of seconds to wait before updating the map", 5.0, 0.001)

# ICP configuration
gen.add("max_alignment_fitness", double_t, 1, "Maximum allowed Euclidean fitness score (sum of squared distances from the source to the target) to perform pose correction", 0.5, 0.0)
gen.add("max_transformation_angle", double_t, 1, "Maximum allowed angle (radians) of transformation pose correction", 0.79, 0.0)
gen.add("max_transformation_distance", double_t, 1, "Maximum allowed distance (meters) of transformation pose correction", 0.5, 0.0)
gen.add("max_correspondence_distance", double_t, 1, "Maximum distance threshold between a point and its nearest neighbor correspondent in order to be considered in the alignment process", 1.0, 0.0)
gen.add("transformation_epsilon", double_t, 1, "Maximum allowable difference between two consecutive transformations in order for an optimization to be considered as having converged to the final solution", 1e-6, 0.0)
gen.add("euclidean_fitness_epsilon", double_t, 1, "Maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the algorithm is considered to have converged", 0.001, 0.0)
gen.add("max_number_of_registration_iterations", int_t, 1, "Maximum number of iterations the internal optimization should run for", 250, 1)
gen.add("max_number_of_ransac_iterations", int_t, 1, "Maximum number of ransac iterations the internal optimization should run for", 100, 0)
gen.add("ransac_outlier_rejection_threshold", double_t, 1, "Maximum allowed Euclidean distance allowed to consider two points as inliers", 0.05, 0.0)

exit(gen.generate(PACKAGE_NAME, NODE_NAME, FILE_NAME))

# gen.add(name, paramtype, level, description, default, min, max, edit_method)